#include "pointMotion.h"


extern unsigned short sysFlags;
extern int ADCvalue;
extern unsigned int	  timerCounterMax;
extern unsigned int	  timerCounter;                     
extern unsigned short pointColisionFlag;

char text[13];
unsigned short pointsNext[9] = {0};	//{pointsNext[0] -> point1 X, pointsNext[1] -> point1 Y, pointsNext[2] -> point1 move status(0,1)}
unsigned short pointSideColisionFlag = 0;
unsigned short rewriteColumns[6];

//******************************************************************************
//
// Initialise start parameters of the points (  coordinates, speeds, move laws)    
//
//******************************************************************************
void pointsInit(void){

	while( (sysFlags&0x01) == 0 );

	srand(ADCvalue);

	points[0].x = rand()%96;
	points[0].y = rand()%16;

	points[1].x = rand()%96;
	points[1].y = rand()%16;

	if( points[0].x == points[1].x && points[0].y == points[1].y ){		//Cant take the same spot like P0
		if( points[1].x < 95 ){
			points[1].x++;
		}else {
			points[1].x--;			
		}
	}		

	points[2].x = rand()%96;
	points[2].y = rand()%16;
	
	if(points[2].x == points[0].x && points[2].y == points[0].y){		//Cant take the same spot like P0
		if( points[2].x < 95 ){
			points[2].x++;
		}else {
			points[2].x--;			
		}
	} 

	if(points[2].x == points[1].x && points[2].y == points[1].y){		//Cant take the same spot like P1
		if( points[2].x < 95 ){
			points[2].x++;
		}else {
			points[2].x--;			
		}
	} 

	points[0].move = moveLaws + (rand()%8);
	points[1].move = moveLaws + (rand()%8);
	points[2].move = moveLaws + (rand()%8);

	
	points[0].speed = (rand()%15) + 3;
	points[1].speed = (rand()%15) + 3;
	points[2].speed = (rand()%15) + 3;

	timerCounterMax = 12252240;		// NOK of all numbers in interval [3,17] the max value of the system timer counter

	pointInitWritePosition();
}

//******************************************************************************
//
// write starts positions in the draw table
//
//******************************************************************************
void pointInitWritePosition(void){
	char mask;

	mask=1;
	mask <<= (points[0].y%8);
	drawTbl[(points[0].y/8)*96 + points[0].x] ^= mask;


	
	mask=1;
	mask <<= (points[1].y%8);
	drawTbl[(points[1].y/8)*96 + points[1].x] ^= mask;


	
	mask=1;
	mask <<= (points[2].y%8);
	drawTbl[(points[2].y/8)*96 + points[2].x] ^= mask;
	
	rewriteColumns[0] = (points[0].y/8)*96 + points[0].x;
	rewriteColumns[1] = (points[1].y/8)*96 + points[1].x;
	rewriteColumns[2] = (points[2].y/8)*96 + points[2].x;
	rewriteColumns[3] = 1;
	rewriteColumns[4] = 1;
	rewriteColumns[5] = 1;
}

//******************************************************************************
//
// Writing the new point position in the draw table and point structure
//
//******************************************************************************
void pointWritePosition(point_t* pPoint, char newX, char newY){
	char mask;
	static short i=0;
	
	mask=1;
	mask <<= (pPoint->y%8);
	drawTbl[(pPoint->y/8)*96 + pPoint->x] ^= mask;
	
	rewriteColumns[i++] = (pPoint->y/8)*96 + pPoint->x;
	if(i>5) i=0;
	
	mask=1;
	mask <<= (newY%8);
	drawTbl[(newY/8)*96 + newX] ^= mask;
	
	rewriteColumns[i++] = (newY/8)*96 + newX;
	if(i>5) i=0;
	
	pPoint->x = newX;
	pPoint->y = newY;
}
//******************************************************************************
//
// Calculates the next positions of the points acordint the curent moving law
//
//******************************************************************************
static void pointNextPos(void){

	//
	// Point 0 is Moveng if animation flag is off and timecounter is devideble by P0Speed cycles
	// if animation flag is on the point wait 1 system timer cycle and then move
	//
	if( sysFlags&0x04 ){
		if( (timerCounter % points[0].speed) == 0 ){
			pointsNext[2] = 0;
		}else {
			pointsNext[2] = 1;
			sysFlags ^= 0x04;	
		}		
	}else if( (timerCounter % points[0].speed) == 0 ){
		pointsNext[2] = 1;
	}else {
		pointsNext[2] = 0;	
	}

	//
	// Point 1 is Moveng if animation flag is off and timecounter is devideble by P0Speed cycles
	// if animation flag is on the point wait 1 system timer cycle and then move
	//
	if( sysFlags&0x08 ){
		if( (timerCounter % points[1].speed) == 0 ){
			pointsNext[5] = 0;
		}else {
			pointsNext[5] = 1;
			sysFlags ^= 0x08;	
		}		
	}else if( (timerCounter % points[1].speed) == 0 ){
		pointsNext[5] = 1;
	}else {
		pointsNext[5] = 0;	
	}

	//
	// Point 2 is Moveng if animation flag is off and timecounter is devideble by P0Speed cycles
	// if animation flag is on the point wait 1 system timer cycle and then move
	//
	if( sysFlags&0x10 ){
		if( (timerCounter % points[2].speed) == 0 ){
			pointsNext[8] = 0;
		}else {
			pointsNext[8] = 1;
			sysFlags ^= 0x10;	
		}		
	}else if( (timerCounter % points[2].speed) == 0 ){
		pointsNext[8] = 1;
	}else {
		pointsNext[8] = 0;	
	}


	if(pointsNext[2] == 1){
		pointsNext[0] = points[0].x + points[0].move->aplToX;
		pointsNext[1] = points[0].y + points[0].move->aplToY;
	}else{
		pointsNext[0] = points[0].x;
		pointsNext[1] = points[0].y;		
	}

	if(pointsNext[5] == 1){
		pointsNext[3] = points[1].x + points[1].move->aplToX;
		pointsNext[4] = points[1].y + points[1].move->aplToY;
	}else{
		pointsNext[3] = points[1].x;
		pointsNext[4] = points[1].y;		
	}

	if(pointsNext[8] == 1){
		pointsNext[6] = points[2].x + points[2].move->aplToX;
		pointsNext[7] = points[2].y + points[2].move->aplToY;
	}else{
		pointsNext[6] = points[2].x;
		pointsNext[7] = points[2].y;		
	}
 
}

//******************************************************************************
//
// check is there a point that want to leave the screen and sets pointSideColisionFlag
//
//******************************************************************************
void pointSideColisionCheck(void){
	
	pointSideColisionFlag = 0;

   	if( pointsNext[2] == 1 )
	{
	  	if(pointsNext[0] > 95){
			pointSideColisionFlag ^= 0x01;					//point1 X out of range
		}
	
		if(pointsNext[1] > 15){
			pointSideColisionFlag ^= 0x02;					//point1 Y out of range
		}
	}

	if( pointsNext[5] == 1 )
	{
		if(pointsNext[3] > 95){
			pointSideColisionFlag ^= 0x04;					//point2 X out of range
		}
	
		if(pointsNext[4] > 15){
			pointSideColisionFlag ^= 0x08;					//point2 Y out of range
		}
	}

	if( pointsNext[8] == 1 )
	{
		if(pointsNext[6] > 95){
			pointSideColisionFlag ^= 0x10;					//point3 X out of range
		}
	
		if(pointsNext[7] > 15){
			pointSideColisionFlag ^= 0x20;					//point3 Y out of range
		}
	}

}

//******************************************************************************
//
// Reverts the point moveing direction 
//
//******************************************************************************
void pointRevertLaw(point_t* pPoint){

	if( pPoint->move == &moveLaws[0] ){
		pPoint->move =  &moveLaws[1];
		return;
	}

	if( pPoint->move == &moveLaws[1] ){
		pPoint->move =  &moveLaws[0];
		return;
	}

	if( pPoint->move == &moveLaws[2] ){
		pPoint->move =  &moveLaws[3];
		return;
	}

	if( pPoint->move == &moveLaws[3] ){
		pPoint->move =  &moveLaws[2];
		return;
	}

	if( pPoint->move == &moveLaws[4] ){
		pPoint->move =  &moveLaws[5];
		return;
	}

	if( pPoint->move == &moveLaws[5] ){
		pPoint->move =  &moveLaws[4];
		return;
	}

	if( pPoint->move == &moveLaws[6] ){
		pPoint->move =  &moveLaws[7];
		return;
	}

	if( pPoint->move == &moveLaws[7] ){
		pPoint->move =  &moveLaws[6];
		return;
	}

}

//******************************************************************************
//
// Corect the moving direction for point that hits left or right sight of the screen
//
//******************************************************************************
void pointSideChangeLaw(point_t* pPoint){

	if(pPoint->move == &moveLaws[4]){
		pPoint->move = &moveLaws[6];
		return;
	}

	if(pPoint->move == &moveLaws[5]){
		pPoint->move = &moveLaws[7];
		return;
	}

	if(pPoint->move == &moveLaws[6]){
		pPoint->move = &moveLaws[4];
		return;
	}

	if(pPoint->move == &moveLaws[7]){
		pPoint->move = &moveLaws[5];
		return;
	}

}

//******************************************************************************
//
// Corect the moving direction for point that hits up or down sight of the screen
//
//******************************************************************************
void pointUpDownChangeLaw(point_t* pPoint){
	
	if(pPoint->move == &moveLaws[4]){
		pPoint->move = &moveLaws[7];
		return;
	}

	if(pPoint->move == &moveLaws[5]){
		pPoint->move = &moveLaws[6];
		return;
	}
	if(pPoint->move == &moveLaws[6]){
		pPoint->move = &moveLaws[5];
		return;
	}

	if(pPoint->move == &moveLaws[7]){
		pPoint->move = &moveLaws[4];
		return;
	}

}

//******************************************************************************
//
// define the side colision and call function to solve it
//
//******************************************************************************
void pointSideColisionSolve(void){

	if( pointSideColisionFlag&0x03 )
	{	
		if( ((points[0].move - &moveLaws[4])<0) || ( (pointSideColisionFlag&0x03) == 0x03) ){
			pointRevertLaw(&points[0]);
	
		}else if(pointSideColisionFlag & 0x01){					
			pointSideChangeLaw(&points[0]);
				
			}else if(pointSideColisionFlag & 0x02 ){
				pointUpDownChangeLaw(&points[0]);
				}
	}

	if( pointSideColisionFlag&0x0C )
	{	
		if( ((points[1].move - &moveLaws[4])<0) || ( (pointSideColisionFlag&0x0C) == 0x0C) )
		{
			pointRevertLaw(&points[1]);
	
		}else if(pointSideColisionFlag & 0x04)
		{
			pointSideChangeLaw(&points[1]);
				
		}else if(pointSideColisionFlag & 0x08)
		{
			pointUpDownChangeLaw(&points[1]);
		}
	}
	
	if( pointSideColisionFlag&0x30 )
	{	
		if( ( (points[2].move - &moveLaws[4])<0 ) || ( (pointSideColisionFlag&0x30) == 0x30 ) )
		{
			pointRevertLaw( &points[2]);
	
		}else if( pointSideColisionFlag & 0x10 )
		{
			pointSideChangeLaw( &points[2] );
				
		}else if( pointSideColisionFlag & 0x20 )
		{
			pointUpDownChangeLaw( &points[2] );
		}
	}

}

//******************************************************************************
//
// calculate the next point positions, define and solve all colisions
//
//******************************************************************************
void pointMove(void){



ReCheck:
	pointNextPos();
	
	if( pointsNext[2] == 0 && pointsNext[5] == 0 && pointsNext[8] == 0 ){
		return;
	}
	
	pointConditionFlagsSet(); 
	pointColisionFlagSet();
	pointSideColisionCheck();

	if( pointSideColisionFlag != 0 ){
	 	pointSideColisionSolve();
	}
	
	if( pointColisionFlag != 0 ){
		CollisionSolver ();
	}
	
	if( pointColisionFlag != 0 || pointSideColisionFlag != 0 ){
		if( sysFlags&0x20 ){
			sysFlags ^= 0x20;
		}else{	
			goto ReCheck;
		}
	}

	if( pointsNext[2] == 1 )
		pointWritePosition(&points[0], pointsNext[0], pointsNext[1]);

	if( pointsNext[5] == 1 )
		pointWritePosition(&points[1], pointsNext[3], pointsNext[4]);

	if( pointsNext[8] == 1 )
		pointWritePosition(&points[2], pointsNext[6], pointsNext[7]);


}
